//
// Created by sundx on 2020/11/9.
//

#ifndef SRC_OFFBOARDMANAGER_H
#define SRC_OFFBOARDMANAGER_H


#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <vector>
#include <iostream>
#include <ctime>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/ExtendedState.h>
#include <fstream>
#include "tf/transform_datatypes.h"
#include <eigen3/Eigen/Core>
#include "px4_gcs/swarm_state.h"
#include "Utils/TrajManager.h"
#include "Utils/CSVManager.h"
#include "Utils/RosUtils.h"
#include "tf/transform_broadcaster.h"

using namespace std;

#define PI 3.1415926


enum air_state {
    in_air,
    landing,
    landed,
    takeoffing,
    waitting_takeoff
};

enum arm_state {
    armed,
    disarmed
};

enum fly_mode {
    offboard,
    manual,
    altctl,
    posctl,
    stabilized
};

enum control_methord {
    pos,
    vel,
    att
};

enum position_state {
    using_vicon,
    using_gps,
    using_att,
};

enum emergence_state{
    emergence,
    normal
};

enum run_state{
    first_takeoff,
    second_wait_swarm,
    third_start_task,
    fourth_land,
};

struct current_state {
    arm_state armState;
    air_state airState;
    control_methord controlMethord;
    position_state positionState;
    fly_mode flyMode;
    emergence_state emergenceState;
    run_state cur;
    run_state next;
    bool current_run_finished;
};

struct pose{
    float x;
    float y;
    float z;
    float roll;
    float pitch;
    float yaw;
};


class OffboardManager {
public:
    string node_name;
    string mavros_id;
    string pkg_path;
    string local_pose_topic_name;
    string publish_pose_topic_name;

    mavros_msgs::PositionTarget sp_pos;
    current_state state{};
    tf::Quaternion cur_att;

    ros::Subscriber vision_pose_sub;
    ros::Subscriber local_vel_sub;
    ros::Subscriber mode_sub;
    ros::Subscriber lan_sub;
    ros::Subscriber swarm_state_sub;
    ros::Publisher sp_raw_local_pub;
    ros::ServiceClient cmd_request;
    mavros_msgs::SetMode mode_cmd;
    mavros_msgs::CommandBool arm_cmd;
    mavros_msgs::ExtendedState land_state;
    ros::ServiceClient arming_client;
    ros::Subscriber local_pose_sub;
    pose last_recorded_pose;

    float cur_pos[3] = {0.0, 0.0, 0.0};
    float cur_vel[3] = {0.0, 0.0, 0.0};
    float cur_angle[3] = {0.0, 0.0, 0.0}; // roll pitch yaw
    float init_pose[3] = {0.0, 0.0, 0.0};
    float sp_rpyt[4] = {0.0, 0.0, 0.0, 0.0};
    float takeoff_height = 0;
    float land_speed = 0.3;
    float init_yaw = 0.0;
    float _target[3] = {0.0, 0.0, 0.0};
    float cur_err_dist = 0.0;

    bool init_process_finished = false;
    bool take_off_finished = false;
    bool land_finished = false;
    bool using_gazebo = false;
    bool use_takeoff = true;
    bool use_land = true;
    bool use_local_head_target = false;

    int id = 0;
    int using_position_methord = 0;//int 1 using_vicon 2 using_gps 3 using_att
    int using_control_mode = 0; // 0 position 1 vel 2 att
    int rate = 30;
    int control_count_time = 0;
    int control_time = 60;

    OffboardManager() = default;

    OffboardManager(ros::NodeHandle &n);

    void initRosSubPubManager(ros::NodeHandle &n);

    void updateParams(ros::NodeHandle &nh);

    void chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg);

    void chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg);

    void chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg);

    void chatterCallback_mode(const mavros_msgs::State &msg);

    void takeoff(float height);

    void holdInitPos(float height);

    void land(float target_x, float target_y);

    void land();

    void holdZeroPosition();

    void initState();

    void moveToRelativeNow(float dx, float dy, float dz);

    void moveto(float target_x, float target_y, float target_z,float base_x,float base_y,float base_z);

    void moveto(float target_x, float target_y, float target_z);

    float getDistanceFrom(float x, float y, float z);

    void try_switch_offboard_armed();

    void setDisarmed();

    void setArmed();

    void setAltctl();

    void setStabilized();

    void setPosctl();

    void setOffboard();

    void record_pose();

    void setMannual();

    void init_position(ros::Rate &loop_rate, int average_num);

    vector<float> getPositionFromTargetBasedOnHome(vector<float> &target);

    std::string stateString(run_state _state);

    void moveByV(float vx, float vy, float vz);

    void rotateYawCCW(float target,float base,float x,float y,float z);

private:

};





#endif //SRC_OFFBOARDMANAGER_H
